
#include "asm.h"

.equ    KERNEL, 0
.equ    USER,   304
.equ    NESTED_COUNT, 608

 
LEAF(asm_start)
    mtc0    zero, CP0_STATUS
    mtc0    zero, CP0_WATCHLO
    mtc0    zero, CP0_WATCHHI

    mfc0    t0, CP0_CONFIG
    and     t0, ~0x7
    ori     t0, 0x2
    mtc0    t0, CP0_CONFIG
    
    jr  ra
END(asm_start)

.macro SAVE_CONTEXT offset
    .set    noat
    ld k0,current_running
    daddi k0,k0,\offset
    sd zero,OFFSET_REG0(k0)
    sd AT,OFFSET_REG1(k0)
    sd v0,OFFSET_REG2(k0)
    sd v1,OFFSET_REG3(k0)
    sd a0,OFFSET_REG4(k0)
    sd a1,OFFSET_REG5(k0)
    sd a2,OFFSET_REG6(k0)
    sd a3,OFFSET_REG7(k0)
    sd t0,OFFSET_REG8(k0)
    sd t1,OFFSET_REG9(k0)
    sd t2,OFFSET_REG10(k0)
    sd t3,OFFSET_REG11(k0)
    sd ta0,OFFSET_REG12(k0)
    sd ta1,OFFSET_REG13(k0)
    sd ta2,OFFSET_REG14(k0)
    sd ta3,OFFSET_REG15(k0)
    sd s0,OFFSET_REG16(k0)
    sd s1,OFFSET_REG17(k0)
    sd s2,OFFSET_REG18(k0)
    sd s3,OFFSET_REG19(k0)
    sd s4,OFFSET_REG20(k0)
    sd s5,OFFSET_REG21(k0)
    sd s6,OFFSET_REG22(k0)
    sd s7,OFFSET_REG23(k0)
    sd t8,OFFSET_REG24(k0)
    sd t9,OFFSET_REG25(k0)
    sd gp,OFFSET_REG28(k0)
    sd sp,OFFSET_REG29(k0)
    sd fp,OFFSET_REG30(k0)
    sd ra,OFFSET_REG31(k0)

    mfc0    k1, CP0_STATUS
    nop
    sw      k1, OFFSET_STATUS(k0)
    mfhi    k1
    sd      k1, OFFSET_HI(k0)
    mflo    k1
    sd      k1, OFFSET_LO(k0)
    dmfc0    k1, CP0_BADVADDR
    nop
    sd      k1, OFFSET_BADVADDR(k0)
    mfc0    k1, CP0_CAUSE
    nop
    sw      k1, OFFSET_CAUSE(k0)
    dmfc0    k1, CP0_EPC
    nop
    sd      k1, OFFSET_EPC(k0)
    .set    at
.endm

.macro RESTORE_CONTEXT offset
    .set    noat
    ld      k0, current_running
	daddi	k0, k0, \offset
    ld      AT, OFFSET_REG1(k0)
    ld      v0, OFFSET_REG2(k0)
    ld      v1, OFFSET_REG3(k0)
	ld      a0, OFFSET_REG4(k0)
	ld      a1, OFFSET_REG5(k0)
	ld      a2, OFFSET_REG6(k0)
	ld      a3, OFFSET_REG7(k0)
	ld      t0, OFFSET_REG8(k0)
	ld      t1, OFFSET_REG9(k0)
	ld      t2, OFFSET_REG10(k0)
	ld      t3, OFFSET_REG11(k0)
	ld      ta0, OFFSET_REG12(k0)
	ld      ta1, OFFSET_REG13(k0)
	ld      ta2, OFFSET_REG14(k0)
	ld      ta3, OFFSET_REG15(k0)
	ld      s0, OFFSET_REG16(k0)
	ld      s1, OFFSET_REG17(k0)
	ld      s2, OFFSET_REG18(k0)
	ld      s3, OFFSET_REG19(k0)
	ld      s4, OFFSET_REG20(k0)
	ld      s5, OFFSET_REG21(k0)
	ld      s6, OFFSET_REG22(k0)
	ld      s7, OFFSET_REG23(k0)
	ld      t8, OFFSET_REG24(k0)
	ld      t9, OFFSET_REG25(k0)
	ld      gp, OFFSET_REG28(k0)
	ld      sp, OFFSET_REG29(k0)
    ld      fp, OFFSET_REG30(k0)
	ld      ra, OFFSET_REG31(k0)
	lw		k1, OFFSET_STATUS(k0)
	mtc0	k1, CP0_STATUS
	ld		k1, OFFSET_HI(k0)
	mthi	k1
	ld		k1, OFFSET_LO(k0)
	mtlo	k1
	ld		k1, OFFSET_BADVADDR(k0)
	dmtc0	k1, CP0_BADVADDR
	lw		k1, OFFSET_CAUSE(k0)
	mtc0	k1, CP0_CAUSE
	ld		k1, OFFSET_EPC(k0)
	dmtc0	k1, CP0_EPC
   .set    at
.endm

# function do_scheduler
NESTED(do_scheduler, 0, ra)
    SAVE_CONTEXT(KERNEL)
    jal scheduler
    RESTORE_CONTEXT(KERNEL)
    jr ra

END(do_scheduler)

LEAF(reset_timer)
    dmtc0 zero,CP0_COUNT
    dmtc0 a0,CP0_COMPARE
    jr ra
END(reset_timer)

LEAF(set_cp0_status)
    dmtc0 a0, CP0_STATUS
    jr ra
END(set_cp0_status)

LEAF(set_cp0_cause)
    dmtc0 a0,CP0_CAUSE
    jr ra
END(set_cp0_cause)

LEAF(get_cp0_status)
   dmfc0 v0,CP0_STATUS
   jr ra 
END(get_cp0_status)

LEAF(get_cp0_cause)
   dmfc0 v0,CP0_CAUSE
   jr ra 
END(get_cp0_cause)

LEAF(get_cp0_count)
    dmfc0 v0,CP0_COUNT
    jr ra
END(get_cp0_count)

LEAF(get_cp0_compare)
	dmfc0 v0,CP0_COMPARE
    jr ra
END(get_cp0_compare)


 
.global exception_handler_begin
.global exception_handler_end
.global TLBexception_handler_begin
.global TLBexception_handler_end

NESTED(TLBexception_handler_entry, 0, sp)
TLBexception_handler_begin:
    ld      k0, current_running
	daddi	k0, k0, 608
    daddi   k1, zero, 1
    ld      k1, 0(k0)
	SAVE_CONTEXT(USER)
    j       handle_tlb
    nop
TLBexception_handler_end:
END(TLBexception_handler_entry)

NESTED(exception_handler_entry, 0, sp)   
exception_handler_begin:
    ld      k0, current_running
	daddi	k0, k0, 608
    daddi   k1, zero, 1
    ld      k1, 0(k0)
    SAVE_CONTEXT(USER)
    dla   k1,exception_handler
    dmfc0 k0,CP0_CAUSE
    andi  k0,k0,CAUSE_EXCCODE
    dsrl  k0,k0,2
    dsll  k0,k0,3
    daddu  k0,k0,k1
    ld    k0,0(k0)
    jr    k0
exception_handler_end:
END(exception_handler_entry)

NESTED(handle_int, 0, sp)
    
    dmfc0   a0, CP0_STATUS
    dmfc0   a1, CP0_CAUSE
    daddi sp,sp,-8
    jal    interrupt_helper
    daddi sp,sp,8
    j exception_handler_exit
END(handle_int)

NESTED(handle_syscall, 0, sp)
   dadd a3, a2, zero
   dadd a2, a1, zero
   dadd a1, a0, zero
   dadd a0, v0, zero
   daddi sp, sp, -16
   jal system_call_helper
   daddi sp, sp, 16
   j    exception_handler_exit
    
END(handle_syscall)

NESTED(handle_tlb, 0, sp) 
    #ld k0,current_running
    #ld sp, OFFSET_REG29(k0)
    jal do_TLB_helper
    j exception_handler_exit
END(handle_tlb)

NESTED(handle_other, 0, sp)
    dmfc0 a0,CP0_CAUSE
    dmfc0 a1,CP0_EPC
    daddi sp,sp,-8
    jal    other_exception_handler
    daddi sp,sp,8
    j exception_handler_exit
END(handle_other)

LEAF(exception_handler_exit)
  RESTORE_CONTEXT(USER)
  dmtc0 zero,CP0_COUNT
   
  eret
END(exception_handler_exit)

LEAF(set_cp0_entryhi)
    mtc0    a0, CP0_ENTRYHI
    jr      ra
END(set_cp0_entryhi)

LEAF(get_cp0_entryhi)
    mfc0    v0, CP0_ENTRYHI
    jr      ra
END(get_cp0_entryhi)

LEAF(get_cp0_index)
    mfc0    v0, CP0_INDEX
    jr      ra    
END(get_cp0_index)

LEAF(set_cp0_index)
    mtc0    a0,CP0_INDEX
    jr      ra
END(set_cp0_index)

LEAF(get_cp0_badvaddr)
    mfc0    v0,CP0_BADVADDR
    jr      ra
END(get_cp0_badvaddr)

LEAF(get_cp0_entrylo0)
    mfc0    v0,CP0_ENTRYLO0
    jr      ra
END(get_cp0_entrylo0)

LEAF(set_cp0_entrylo0)
    mtc0    a0,CP0_ENTRYLO0
    jr      ra
END(set_cp0_entrylo0)

LEAF(get_cp0_entrylo1)
    mfc0    v0,CP0_ENTRYLO1
    jr      ra
END(get_cp0_entrylo1)

LEAF(set_cp0_entrylo1)
    mtc0    a0,CP0_ENTRYLO1
    jr      ra
END(set_cp0_entrylo1)

LEAF(set_cp0_pagemask)
    mtc0    a0,CP0_PAGEMASK
    jr      ra
END(set_cp0_pagemask)

LEAF(get_cp0_context)
    mfc0    a0,CP0_CONTEXT
    jr      ra
END(get_cp0_context)

LEAF(tlbwr_operation)
   tlbwr
   jr ra
END(tlbwr_operation)

LEAF(tlbwi_operation)
   tlbwi 
   jr ra
END(tlbwi_operation)

LEAF(tlbp_operation)
   tlbp 
   jr ra
END(tlbp_operation)



LEAF(get_cp0_config)
    
END(get_cp0_config)

LEAF(get_cpu_id)
    
END(get_cpu_id)

